#include "ArmorCalculator.h"

namespace hitcrt {
/**
 * @brief
 * @param[in] armors         MyParam doc
 * @param[in] frame          MyParam doc
 * @author fx_03 (18249293810@qq.com)
 */
void ArmorCalculator::rotationCalculation(std::vector<Armor>& armors,
                                          cv::Mat& frame) {
    for (int i = 0; i < armors.size(); i++) {
        // 创建世界坐标系中的角点
        getCornerPoint(i, armors);

        bool success =
            cv::solvePnP(m_pointCornerWorld, m_pointCornerImg, m_cameraMatrix,
                         m_distCoeffs, m_rvec, m_tvec, 0, 6);

        if (success) {
            // std::cout << m_tvec << std::endl;
            rotationTransalion(m_tvec, m_rvec, armors[i]);
            resultVersion(i, armors, frame);
        }
    }
}
/**
 * @brief 传入yaw和pitch角 对装甲板坐标系 即中心坐标转换
 * @param[in] tvec           MyParam doc
 * @param[in] revc           MyParam doc
 * @param[in] armor          MyParam doc
 * @author fx_03 (18249293810@qq.com)
 */
void ArmorCalculator::rotationTransalion(cv::Mat tvec, cv::Mat rvec,
                                         Armor& armor) {
    cv::Mat wldInCam = cv::Mat::zeros(4, 4, CV_64FC1);
    cv::Rodrigues(rvec, wldInCam(cv::Rect(0, 0, 3, 3)));
    tvec.copyTo(wldInCam(cv::Rect(3, 0, 1, 3)));
    // 所求为像素坐标系对于世界坐标系的转换
    cv::Mat camInWld = wldInCam.inv();
    Eigen::Matrix4d translation;
    translation << wldInCam.at<double>(0, 0), wldInCam.at<double>(0, 1),
        wldInCam.at<double>(0, 2), wldInCam.at<double>(0, 3),
        wldInCam.at<double>(1, 0), wldInCam.at<double>(1, 1),
        wldInCam.at<double>(1, 2), wldInCam.at<double>(1, 3),
        wldInCam.at<double>(2, 0), wldInCam.at<double>(2, 1),
        wldInCam.at<double>(2, 2), wldInCam.at<double>(2, 3),
        wldInCam.at<double>(3, 0), wldInCam.at<double>(3, 1),
        wldInCam.at<double>(3, 2), wldInCam.at<double>(3, 3);

    m_pointCenterImg =
        cv::Point3f(m_tvec.at<double>(0, 0), m_tvec.at<double>(0, 1),
                    m_tvec.at<double>(0, 2));
    // 像素坐标系到云台坐标系的平移变换

    m_pointCenterImg.z = m_pointCenterImg.z + m_shiftZ;
    m_pointCenterImg.y = m_pointCenterImg.y + m_shiftY;
    // 注意这里传入的角度是什么
    Eigen::Matrix3d Ry, Rx, R;
    Eigen::Vector3d centerTemp;
    Ry << cos(m_yawCameraToRobot/180*CV_PI), 0, sin(m_yawCameraToRobot/180*CV_PI), 0, 1, 0,
        -sin(m_yawCameraToRobot/180*CV_PI), 0, cos(m_yawCameraToRobot/180*CV_PI);
    Rx << 1,0,0,0,cos(m_pitchCameraToRobot/180*CV_PI), -sin(m_pitchCameraToRobot/180*CV_PI), 0,
        sin(m_pitchCameraToRobot/180*CV_PI), cos(m_pitchCameraToRobot/180*CV_PI);
    R = Ry * Rx;
    std::cout<<R<<std::endl;
    //std::cout<<R<<std::endl;
    centerTemp << m_pointCenterImg.x, m_pointCenterImg.y, m_pointCenterImg.z;
    // std::cout << "centerTemp" << centerTemp << std::endl;

    centerTemp = R * centerTemp;

    m_pointCenterCar = cv::Point3f(centerTemp(0), centerTemp(1), centerTemp(2));
    armor.m_measurePoint = m_pointCenterCar;


    m_yawSend = std::atan(std::abs(m_pointCenterCar.x)/std::abs(m_pointCenterCar.z));
    m_pitchSend = std::atan(std::abs(m_pointCenterCar.z)/std::abs(m_pointCenterCar.y));
    armor.m_yawSend = m_yawSend;
    armor.m_pitchSend = m_pitchSend;
    // std::cout << "m_pointCenterImg:" << std::endl;
    // std::cout << m_pointCenterCar << std::endl;
}

/**
 * @brief 清空vector成员变量
 * @author fx_03 (18249293810@qq.com)
 */
void ArmorCalculator::clearVectors() {
#pragma omp parallel for num_threads(12)
    m_pointCornerImg.clear();
    m_pointCornerImg.shrink_to_fit();
    m_pointCornerWorld.clear();
    m_pointCornerWorld.shrink_to_fit();
}

void ArmorCalculator::getCornerPoint(int index, std::vector<Armor>& armors) {
    /*获取中心点旋转矩阵和位置*/
    // 获取装甲板四个角点的像素坐标  // 顺时针

    // 左上
    m_pointCornerImg.emplace_back(
        cv::Point2f(armors[index].m_topLeft.x, armors[index].m_topLeft.y));
    // 右上
    m_pointCornerImg.emplace_back(
        cv::Point2f(armors[index].m_topRight.x, armors[index].m_topRight.y));
    // 右下
    m_pointCornerImg.emplace_back(cv::Point2f(armors[index].m_bottomRight.x,
                                              armors[index].m_bottomRight.y));
    // 左下
    m_pointCornerImg.emplace_back(cv::Point2f(armors[index].m_bottomLeft.x,
                                              armors[index].m_bottomLeft.y));

    m_width = (armors[index].m_size == SMALL) ? 135.0 : 230.0;
    m_height = 55.0;
    //  获取装甲板四个角点的世界坐标

    // 顺时针
    // 左上
    m_pointCornerWorld.emplace_back(
        cv::Point3f(-m_width / 2.0, -m_height / 2.0, 0));
    // 右上
    m_pointCornerWorld.emplace_back(
        cv::Point3f(m_width / 2.0, -m_height / 2.0, 0));
    // 右下
    m_pointCornerWorld.emplace_back(
        cv::Point3f(m_width / 2.0, m_height / 2.0, 0));
    // 左下
    m_pointCornerWorld.emplace_back(
        cv::Point3f(-m_width / 2.0, m_height / 2.0, 0));
}
void ArmorCalculator::resultVersion(int index, std::vector<Armor>& armors,
                                    cv::Mat& frame) {
    // std::string strPointCornerWorld =
    //     "X: " + std::to_string((int)m_pointCenterImg.back()[0]) +
    //     " Y: " + std::to_string((int)m_pointCenterImg.back()[1]) +
    //     " Z: " + std::to_string((int)m_pointCenterImg.back()[2]);
    //  反解坐标轴 为可视化做准备
    std::vector<cv::Point3f> pointWorldAxis(4);
    // 原点
    pointWorldAxis[0].x = 0.;
    pointWorldAxis[0].y = 0.;
    pointWorldAxis[0].z = 0;
    // X轴
    pointWorldAxis[1].x = 50;
    pointWorldAxis[1].y = 0.;
    pointWorldAxis[1].z = 0;
    // Y轴
    pointWorldAxis[2].x = 0.;
    pointWorldAxis[2].y = 50;
    pointWorldAxis[2].z = 0;
    // Z轴
    pointWorldAxis[3].x = 0.;
    pointWorldAxis[3].y = 0.;
    pointWorldAxis[3].z = 50;

    std::vector<cv::Point2f> pointImgAxis(4);
    cv::Mat R;
    cv::Rodrigues(m_rvec, R);

    /*装甲板大小类型可视化*/
    { /*
         std::string sizeTxt =
             (armors[index].m_size == SMALL) ? "SMALL" : "LARGE";

         // 将 cv::Mat 转换为字符串
         cv::putText(frame, sizeTxt, armors[index].m_topRight,
                     cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 1);
         cv::putText(frame, strPointCornerWorld, cv::Point(10, 85 + 30 * index),
                     cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 1);
     */
    }

    std::string xCeneterCalculator = std::to_string((int)m_pointCenterCar.x);
    std::string yCeneterCalculator = std::to_string((int)m_pointCenterCar.y);
    std::string zCeneterCalculator = std::to_string((int)m_pointCenterCar.z);

    // 可视化滤波前的点
    cv::putText(frame, xCeneterCalculator, cv::Point2f(150, 55),
                cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 0, 255), 1);
    cv::putText(frame, yCeneterCalculator, cv::Point2f(150, 80),
                cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 0, 255), 1);
    cv::putText(frame, zCeneterCalculator, cv::Point2f(150, 105),
                cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 0, 255), 1);

    cv::projectPoints(pointWorldAxis, m_rvec, m_tvec, m_cameraMatrix, m_distCoeffs,
                      pointImgAxis);

    /*可视化*/
    {
        cv::circle(frame, pointImgAxis[0], 4, cv::Scalar(0, 0, 255), -1, 8);

         cv::line(frame, pointImgAxis[0], pointImgAxis[1], cv::Scalar(0, 255,
         0), 1, 8);
          cv::line(frame, pointImgAxis[0], pointImgAxis[2],
         cv::Scalar(0, 0, 255), 1, 8);
         cv::line(frame, pointImgAxis[0],
         pointImgAxis[3], cv::Scalar(255, 0, 0), 1, 8);

        //  cv::line(frame, pointImgAxis[0], pointImgAxis[1],
        //           cv::Scalar(0, 255, 255), 1, 8);
        //  cv::line(frame, pointImgAxis[1], pointImgAxis[2],
        //           cv::Scalar(0, 255, 255), 1, 8);
        //  cv::line(frame, pointImgAxis[2], pointImgAxis[3],
        //           cv::Scalar(0, 255, 255), 1, 8);
        //  cv::line(frame, pointImgAxis[3], pointImgAxis[0],
        //           cv::Scalar(0, 255, 255), 1, 8);
    }
}
}  // namespace hitcrt
